#include "stdafx.h"
#include "Quaternion.h"

Quaternion::Quaternion()
{
	X=0.0f;
	Y=0.0f;
	Y=0.0f;
	W=1.0f;
}

Quaternion::Quaternion( const float _W, const float _x, const float _y, const float _z )
{
	X=_x;
	Y=_y;
	Z=_z;
	W=_W;
}

Quaternion::Quaternion( const float _W, const Vector3 &vec )
{
	X=vec.x;
	Y=vec.y;
	Z=vec.z;
	W=_W;
}

Quaternion::Quaternion( const float theta_z, const float theta_y, const float theta_x )
{
	float cos_z_2 = (float)cos(0.5*theta_z);
	float cos_y_2 = (float)cos(0.5*theta_y);
	float cos_x_2 = (float)cos(0.5*theta_x);

	float sin_z_2 = (float)sin(0.5*theta_z);
	float sin_y_2 = (float)sin(0.5*theta_y);
	float sin_x_2 = (float)sin(0.5*theta_x);

	// and now compute Quaternion
	W   = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
	X = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
	Y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
	Z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
}

Quaternion::Quaternion( const Vector3 &angles )
{

	float cos_z_2 = (float)cos(0.5*angles.z);
	float cos_y_2 = (float)cos(0.5*angles.y);
	float cos_x_2 = (float)cos(0.5*angles.x);

	float sin_z_2 = (float)sin(0.5*angles.z);
	float sin_y_2 = (float)sin(0.5*angles.y);
	float sin_x_2 = (float)sin(0.5*angles.x);

	// and now compute Quaternion
	Z   = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
	X = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
	Y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
	Z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;	
}

Quaternion & Quaternion::operator=( const Quaternion &q )
{
	W= q.W; 
	X= q.X; 
	Y= q.Y;
	Z= q.Z;
	return *this;
}

Quaternion Quaternion::operator+( const Quaternion &q ) const
{
	 return Quaternion(W+q.W, X+q.X,Y+q.Y,Z+q.Z);
}

Quaternion Quaternion::operator-( const Quaternion &q ) const
{
	 return Quaternion(W-q.W, X-q.X,Y-q.Y,Z-q.Z);
}

Quaternion Quaternion::operator*( const Quaternion &q ) const
{
	float w,x,y,z;

	w = W*q.W - (X*q.X + Y*q.Y + Z*q.Z);

	x = W*q.X + q.W*X + Y*q.Z - Z*q.Y;
	y = W*q.Y + q.W*Y + Z*q.X - X*q.Z;
	z = W*q.Z + q.W*Z + X*q.Y - Y*q.X;

	return Quaternion(w,x,y,z);  
}
Quaternion Quaternion::operator/( const Quaternion &q ) const
{
	Quaternion p(q); 
	p.invert(); 
	return *this * p;
}

const Quaternion Quaternion::operator*( float scale ) const
{
	return Quaternion(W*scale,X*scale,Y*scale,Z*scale);
}
const Quaternion Quaternion::operator/( float scale ) const
{
	return Quaternion(W/scale,X/scale,Y/scale,Z/scale);
}

Quaternion Quaternion::operator-() const
{
	return Quaternion(-W, -X,-Y,-Z);
}

Quaternion &Quaternion::operator+=( const Quaternion &q )
{
	W+=q.W; 
	X+=q.X;
	Y+=q.Y;
	Z+=q.Z;
	return *this;
}

Quaternion & Quaternion::operator-=( const Quaternion &q )
{
	W-=q.W; 
	X-=q.X;
	Y-=q.Y;
	Z-=q.Z;
	return *this;
}

Quaternion & Quaternion::operator*=( const Quaternion &q )
{
	float w,x,y,z;

	w = W*q.W - (X*q.X + Y*q.Y + Z*q.Z);

	x = W*q.X + q.W*X + Y*q.Z - Z*q.Y;
	y = W*q.Y + q.W*Y + Z*q.X - X*q.Z;
	z = W*q.Z + q.W*Z + X*q.Y - Y*q.X;
	W=w;
	X=x;
	Y=y;
	Z=z;
	return *this;
}

Quaternion & Quaternion::operator*=( const float scale )
{
	 X*=scale;
	 Y*=scale;
	 Z*=scale;
	 W*=scale; 
	 return *this;
}

const Quaternion & Quaternion::operator/=( const float scale )
{
	if(scale!=0)
	{
		X/=scale;
		Y/=scale;
		Z/=scale;
		W/=scale;
	}
	return *this;
}

float Quaternion::length() const
{
	return (float)sqrt(W*W+X*X+Y*Y+Z*Z);
}

float Quaternion::length_Squared() const
{
	return (W*W+X*X+Y*Y+Z*Z);
}

void Quaternion::normalize()
{
	*this/=length();
}

Quaternion Quaternion::normalized() const
{
	return  *this/length(); 
}

void Quaternion::conjugate()
{
	X=-X;
	Y=-Y;
	Z=-Y;

}
void Quaternion::invert()
{
	conjugate(); 
	*this/=length_Squared();
}

Quaternion Quaternion::logarit() const
{
	float a = (float)acos(W);
	float sina = (float)sin(a);
	Quaternion ret;

	ret.W = 0;
	if (sina > 0)
	{
		ret.X = a*X/sina;
		ret.X = a*Y/sina;
		ret.X = a*Z/sina;
	} else {
		ret.X= ret.Y= ret.Z= 0;
	}
	return ret;
}	

Quaternion Quaternion::exp() const
{

	float a = (float)sqrt(X*X+Y*Y+Z*Z);
	float sina = (float)sin(a);
	float cosa = (float)cos(a);
	Quaternion ret;

	ret.W = cosa;
	if (a > 0)
	{
		ret.X = sina * X / a;
		ret.Y = sina * Y / a;
		ret.Z = sina * Z / a;
	} else {
		ret.X = ret.Y = ret.Z = 0;
	}
	return ret;
}

Matrix4 Quaternion::toMatrix4() const
{
	//return Matrix4(
	//	W,  -X, -Y,-Z,
	//	X,  W,  -Z, Y,
	//	Y, Z,    W,-X,
	//	Z,-Y,  X,   W);
	float xx      = X * X;
	float xy      = X * Y;
	float xz      = X * Z;
	float xw      = X * W;
	float yy      = Y * Y;
	float yz      = Y * Z;
	float yw      = Y * W;
	float zz      = Z * Z;
	float zw      = Z * W;
	float mat[16];
	mat[0]  = 1 - 2 * ( yy + zz );
	mat[4]  =     2 * ( xy - zw );
	mat[8]  =     2 * ( xz + yw );
	mat[1]  =     2 * ( xy + zw );
	mat[5]  = 1 - 2 * ( xx + zz );
	mat[9]  =     2 * ( yz - xw );
	mat[2]  =     2 * ( xz - yw );
	mat[6]  =     2 * ( yz + xw );
	mat[10] = 1 - 2 * ( xx + yy );
	mat[12]  = mat[13] = mat[14] = mat[3] = mat[7] = mat[11] = 0;
	mat[15] = 1;
	return Matrix4(mat);
}
Vector3 Quaternion::euler_angles() const
{
	float sqw = W*W;    
	float sqx = X*X;    
	float sqy = Y*Y;    
	float sqz = Z*Z;    

	Vector3 euler;
	euler.x = atan2f(2.f * (X*Y + Z*W), sqx - sqy - sqz + sqw);    		
	euler.y = asinf(-2.f * (X*Z - Y*W));
	euler.z = atan2f(2.f * (Y*Z + X*W), -sqx - sqy + sqz + sqw);    
	return euler;
}

Vector3 Quaternion::rotate( const Vector3 &v )
{   
	Quaternion V(0, v);
	Quaternion conjugate(*this);
	conjugate.conjugate();
	Quaternion q(*this * V * conjugate);
	return Vector3(q.X,q.Y,q.Z);
}
void Quaternion::to_axis_angle( Vector3 &axis, float &angle ) const
{
	angle = acosf(W);

	// pre-compute to save time
	float sinf_theta_inv = 1.0f/sin(angle);

	// now the vector
	axis.x = X*sinf_theta_inv;
	axis.y = Y*sinf_theta_inv;
	axis.z = Z*sinf_theta_inv;

	// multiply by 2
	angle*=2;
}
Quaternion Quaternion::slerp( const Quaternion &q1, const Quaternion &q2, float t )
{
	Quaternion q3;
	float dot = Quaternion::dot(q1, q2);

	/*	dot = cos(theta)
		if (dot < 0), q1 and q2 are more than 90 degrees apart,
		so we can invert one to reduce spinning	*/
	if (dot < 0)
	{
		dot = -dot;
		q3 = -q2;
	} else q3 = q2;
	
	if (dot < 0.95f)
	{
		float angle = acosf(dot);
		return (q1*sinf(angle*(1-t)) + q3*sinf(angle*t))/sinf(angle);
	} else // if the angle is small, use linear interpolation								
		return lerp(q1,q3,t);
}

Quaternion Quaternion::slerpNoInvert( const Quaternion &q1, const Quaternion &q2, float t )
{
	float dot = Quaternion::dot(q1, q2);

	if (dot > -0.95f && dot < 0.95f)
	{
		float angle = acosf(dot);			
		return (q1*sinf(angle*(1-t)) + q2*sinf(angle*t))/sinf(angle);
	} else  // if the angle is small, use linear interpolation								
		return lerp(q1,q2,t);			
}

Quaternion Quaternion::squad( const Quaternion &q1,const Quaternion &q2,const Quaternion &a,const Quaternion &b,float t )
{
	Quaternion c= slerpNoInvert(q1,q2,t),
		d= slerpNoInvert(a,b,t);		
	return slerpNoInvert(c,d,2*t*(1-t));
}

Quaternion Quaternion::bezier( const Quaternion &q1,const Quaternion &q2,const Quaternion &a,const Quaternion &b,float t )
{
	Quaternion q11= slerpNoInvert(q1,a,t),
		q12= slerpNoInvert(a,b,t),
		q13= slerpNoInvert(b,q2,t);		
	// level 2 and 3
	return slerpNoInvert(slerpNoInvert(q11,q12,t), slerpNoInvert(q12,q13,t), t);
}

Quaternion Quaternion::spline( const Quaternion &qnm1,const Quaternion &qn,const Quaternion &qnp1 )
{
	Quaternion qni(qn.W, -qn.X,-qn.Y,-qn.Z);	
	return qn * (( (qni*qnm1).logarit()+(qni*qnp1).logarit() )/-4).exp();
}

Quaternion Quaternion::from_axis_angle( const float angle,const Vector3 &axis)
{
	Vector3 aa=axis;
	//aa.normalize();
	Vector3 vec=aa*sinf(angle/2);
	Quaternion a(cosf(angle/2), vec.x,vec.y,vec.z);
	//a.normalize();
	return a;
}

Quaternion Quaternion::from_axis_angle( const float angle,const float XAxis,const float YAxis,const float ZAxis )
{
	Vector3 vec;
	vec.x=sinf(angle/2)*XAxis;
	vec.y=sinf(angle/2)*YAxis;
	vec.z=sinf(angle/2)*ZAxis;
	return Quaternion(cosf(angle/2), vec.x,vec.y,vec.z);
}

Quaternion Quaternion::lerp( const Quaternion &q1, const Quaternion &q2, float t )
{ 
	return (q1*(1-t) + q2*t).normalized(); 
}




